#pragma once

#include <vector>
#include <string>
#include <iterator>
#include <map>
#include "stdio.h"
#include <iostream>
#include <fstream>
#include "stdlib.h"
using namespace std;

#define DEFAULT_LANE_WIDTH 10.0  // meter
#define DEFAULT_ARRAY_SIZE 80
#define WAYPT_NAME_SIZE 20
typedef struct {char str[WAYPT_NAME_SIZE];} waypt_name_t;

enum Lane_marking
    {DOUBLE_YELLOW, SOLID_YELLOW, SOLID_WHITE, BROKEN_WHITE, UNDEFINED};



enum RNDF_PARSE_STATE {COMMENT, GENERAL, SEGMENTS, LANES,
ZONES, PERIMETER, PARKING_SPOT, UNKNOWN};

enum MDF_PARSE_STATE {MDF_COMMENT, MDF_GENERAL, MDF_CHECKPOINTS,
MDF_SPEEDLIMITS, MDF_UNKNOWN};


class LatLong
    {
    public:
        double latitude;
        double longitude;

        // void constructor
        LatLong(void)
            {
            latitude = longitude = 0.0;
            }

        // initialization constructor
        LatLong(double lat, double lon)
            {
            latitude = lat;
            longitude = lon;
            }
        bool operator==(const LatLong &that)
            {
            bool match = (this->latitude == that.latitude && this->longitude == that.longitude);
            //printf("%s\n", match?"Match":"No Match");
            return (match);
            }
    };

class ElementID
    {
    public:
        int seg;
        int lane;
        int pt;

        // constructors
        ElementID()
            {
            seg = lane = pt = -1;
            };
        ElementID(int _seg, int _lane, int _pt)
            {
            seg = _seg;
            lane = _lane;
            pt = _pt;
            };


        bool operator==(const ElementID &that) const
            {
            return (this->seg == that.seg
                && this->lane == that.lane
                && this->pt == that.pt);
            }

        bool operator!=(const ElementID &that) const
            {
            return (this->seg != that.seg
                || this->lane != that.lane
                || this->pt != that.pt);
            }

        bool operator<(const ElementID &that) const
            {
            if (this->seg != that.seg)
                return this->seg < that.seg;
            else if (this->lane != that.lane)
                return this->lane < that.lane;
            else return this->pt < that.pt;
            }

        bool operator>(const ElementID &that) const
            {
            if (this->seg != that.seg)
                return this->seg > that.seg;
            else if (this->lane != that.lane)
                return this->lane > that.lane;
            else return this->pt > that.pt;
            }

        bool valid() const
            {
            return (seg >= 0 && lane >= 0 && pt >= 0);
            }

        bool same_lane(int segid, int laneid) const
            {
            return (seg == segid && lane == laneid);
            };

        bool same_lane(ElementID wayid) const
            {
            return (seg == wayid.seg && lane == wayid.lane);
            };

    };


class WayPointNode			//< way-point graph node
    {
    public:
        LatLong ll;				//< latitude and longitude
        ElementID id;				//< way-point ID
        int index;			//< parser index of waypoint

        // way-point flags
        bool is_entry;			//< lane or zone exit point
        bool is_exit;				//< lane or zone entry point
        bool is_lane_change;			//< change lanes after here
        int checkpoint_id;			//< checkpoint ID or zero
        float lane_width;

        // constructors
        WayPointNode(){ clear();};

        
        // public methods
        void clear()
            {
            index = 0;
            lane_width=0;
            id = ElementID();
            };

        bool operator<(const WayPointNode &that)
            {
            return (this->id < that.id);
            };

        bool operator==(const WayPointNode &that)
            {
            return (this->ll == that.ll &&
                this->id == that.id &&
                this->index == that.index &&
                this->lane_width == that.lane_width);
            };
    };


template <class T>
void print_vector (std::vector<T> vec);

//<waypoint>
class LL_Waypoint 
    {
 public:
  int waypoint_id; //integer > 0
  LatLong ll;
  //double latitude; //6 decimal digits
  //double longitude; //6 decimal digits
  
  //METHODS
  LL_Waypoint(std::string line, int x, int y, int line_number, bool& valid,
	      bool verbose);
  bool isvalid(){return(waypoint_id > 0);};
  void clear(){ waypoint_id = -1; ll.latitude = ll.longitude = -1.0;};
  void print(){print_without_newline(); printf("\n");};
  void print_without_newline()
      {
    printf("Waypoint %d, Latitude: %f, Longitude: %f", 
	   waypoint_id, ll.latitude, ll.longitude);
  };
};

typedef LL_Waypoint Perimeter_Point;

//<checkpoint>
class Checkpoint 
    {
 public:
  int checkpoint_id; //integer > 0
  int waypoint_id;   //integer > 0

  //METHODS
  Checkpoint(){};
  //Returns a parsed checkpoint from 'line' with waypoint id 'x.y.z' 
  Checkpoint (std::string line, int x, int y, int line_number, bool& valid,
	      bool verbose);
  bool isvalid(){return (waypoint_id > 0 && checkpoint_id > 0 );};
  void clear(){ waypoint_id = checkpoint_id = -1;}; 
  void print(){
    printf("Checkpoint Number %d is at Waypoint %d\n", 
	   checkpoint_id, waypoint_id);
  };
};

class Unique_id
    {
 public:
  int waypoint_id;
  int lane_id;
  int segment_id;
 
  //METHODS
  bool isvalid(){return (waypoint_id > 0 && lane_id >= 0 && segment_id > 0);};
  void clear();
  void print(){printf("%d.%d.%d", segment_id, lane_id, waypoint_id);};
};
//<exit>
class Exit {
 public:
  Unique_id start_point;
  Unique_id end_point;
  Exit(std::string line, int x, int y, int line_number, bool& valid, 
       bool verbose);
  bool isvalid(){    return (start_point.isvalid() && end_point.isvalid());};
  void clear(){start_point.waypoint_id = end_point.waypoint_id
      = end_point.lane_id = end_point.segment_id = -1;};
  void print();
};


//<stop>
class Stop{
 public:
  int waypoint_id;
  bool isvalid(){return (waypoint_id > 0);};
  void clear(){waypoint_id = -1;};
  void print(){printf("Stop at Waypoint %d\n", waypoint_id);}; 
  //Returns a parsed stop from 'line' with waypoint id 'x.y.z' 
  Stop (std::string line, int x, int y, int line_number, bool& valid, bool verbose);
};

//<lane>
class Lane{
public:
    int lane_id; //integer > 0
    int number_of_waypoints; //integer > 0
    std::vector<LL_Waypoint> waypoints; //List of waypoints
    //<optional lane header>
    int lane_width; //integer >= 0 (OPTIONAL)
    Lane_marking left_boundary;
    Lane_marking right_boundary;
    std::vector<Checkpoint> checkpoints;
    std::vector<Stop> stops;
    std::vector<Exit> exits;

    //METHODS
    bool isvalid(){return (lane_id > 0 && number_of_waypoints > 0 
        && lane_width >= 0
        && number_of_waypoints == (int)waypoints.size());};
    void clear();
    void print();
    };

//<segment>
class Segment{
 public:
  int segment_id; // integer > 0
  int number_of_lanes; // integer > 0
  //<optional segment header>
  std::string segment_name; // such as "Wisconsin_Ave"
  std::vector<Lane> lanes; // List of Lanes
 
  //METHODS
  bool isvalid(){return (segment_id > 0 && number_of_lanes > 0
			 && number_of_lanes == (int)lanes.size());};
  void clear();
  void print();
};
 

class RNDF {
 public:
  //ELEMENTS
  std::string filename; //filename
  int number_of_segments; //number of segments (integer > 0)
  int number_of_zones; //number of zones (integer >= 0)

  //<optional file header>
  std::string format_version;
  std::string creation_date;

  std::vector<Segment> segments;

  //METHODS
  RNDF(std::string rndfname, bool verbose=false);
  ~RNDF() {};

  //void populate_graph(Graph& graph);
  void print();

  bool is_valid;

 //private:
  bool isvalid()
      {return(number_of_segments > 0 && number_of_zones >= 0
			&& number_of_segments == (int) segments.size());};

  struct id_comparator 
      {
      bool operator()(const ElementID e1, const ElementID e2) const
          {
          if (e1.seg != e2.seg)
              return e1.seg < e2.seg;
          else if (e1.lane != e2.lane)
              return e1.lane < e2.lane;
          else return e1.pt < e2.pt;
          }
      };
  typedef std::map<ElementID, WayPointNode, id_comparator> id_to_waypoint_map;
    
  id_to_waypoint_map id_map;

  // Exit position, which is the signal head position
 typedef std::map<ElementID, LatLong, id_comparator> exit_to_waypoint_map;
  exit_to_waypoint_map exit_map;  
  // The phase number of a segment.lane
 typedef std::map<ElementID, int, id_comparator> phase_to_waypoint_map;
  phase_to_waypoint_map phaseNo_map;// HERE: in elementID the waypoint.id must be 1.



  //std::vector<WayPointEdge> edges;

  void prep_graph();

  int line_number;
  
};


//Global Functions
std::string parse_string(std::string line, std::string token, 
                         int line_number, bool& valid, bool verbose);
int parse_integer(std::string line, std::string token, 
                  int line_number, bool& valid, bool verbose);
int parse_integer(std::string line, int line_number, bool& valid, bool verbose);

void exit_error(Exit& exit);
void print_error_message(int line_number, std::string token);


